﻿using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using System.Text;

namespace ImageTour
{
    class Quad
    {
        internal Float2 P1, P2, P3, P4;
        internal Float3 V1, V2, V3, V4;
    }
    class PerspectiveTransformation
    {
        public float horizon, camHeight, focalLen, eyeX, eyeY;
        public Matrix4 projection, modelView, transform;

        internal PerspectiveTransformation(float horizon, float camHeight, float focalLen, float eyeX, float eyeY)
        {
            this.horizon = horizon;
            this.camHeight = camHeight;
            this.focalLen = focalLen;
            this.eyeX = eyeX;
            this.eyeY = eyeY;
            float viewTheta = (float)Math.Atan((horizon - eyeY) / focalLen);
            float viewY = (horizon - eyeY) / focalLen;
            Float3 view = new Float3(0.0f, -viewY, -1.0f);
            view = view.Normalize();
            Float3 up = Float3.Cross(new Float3(1.0f, 0.0f, 0.0f), view);
            Matrix4 translation = Matrix4.Translate(0.0f, -camHeight, 0.0f);

            modelView = new Matrix4();
            modelView.M[0] = 1.0f; modelView.M[1] = 0.0f; modelView.M[2] = 0.0f; modelView.M[3] = 0.0f;
            modelView.M[4] = up.x; modelView.M[5] = up.y; modelView.M[6] = up.z; modelView.M[7] = 0.0f;
            modelView.M[8] = -view.x; modelView.M[9] = -view.y; modelView.M[10] = -view.z; modelView.M[11] = 0.0f;
            modelView.M[12] = 0.0f; modelView.M[13] = 0.0f; modelView.M[14] = 0.0f; modelView.M[15] = 1.0f;

            modelView = modelView * translation;

            projection = Matrix4.Perspective((float)(2 * Math.Atan(eyeY / focalLen)), eyeX / (float)eyeY, 1.0f, 10000.0f);

            transform = projection * modelView;
        }
        internal Float2 ProjectPoint(Float3 p)
        {
            Float3 rs = transform.Transform(p);
            return new Float2(rs.x, -rs.y);
        }
        internal Float3 UnprojectBasepoint(Float2 p)
        {
            Float3 rs;
            float dy1 = (horizon - p.y) / focalLen;
            rs.z = camHeight / dy1;
            rs.x = -(p.x - eyeX) / focalLen * rs.z;
            rs.y = 0.0f;
            return rs;
        }

        internal Float3 UnprojectCeilpoint(Float2 p, float wallHeight)
        {
            Float3 rs;
            float dy1 = (horizon - p.y) / focalLen;
            rs.z = -(wallHeight - camHeight) / dy1;
            rs.x = -(p.x - eyeX) / focalLen * rs.z;
            rs.y = wallHeight;
            return rs;
        }
    }
    class LineAnnotation
    {
        internal Point P1, P2;
        internal bool IsAux = false;
        internal Point Center
        {
            get
            {
                return new Point((P1.X + P2.X) / 2, (P1.Y + P2.Y) / 2);
            }
        }
        internal Float3 Equation
        {
            get
            {
                var dir = new Float2(P1) - new Float2(P2);
                var norm = new Float2(-dir.y, dir.x);
                var c = -(norm * new Float2(P1));
                return new Float3(norm.x, norm.y, c);
            }
        }
        internal Quad ToQuad(PerspectiveTransformation trans, float wallHeight, int imgW, int imgH)
        {
            Quad rs = new Quad();
            rs.P1 = new Float2(P1);
            rs.P2 = new Float2(P2);
            rs.V1 = trans.UnprojectBasepoint(rs.P1);
            rs.V2 = trans.UnprojectBasepoint(rs.P2);
            rs.V3 = rs.V2 + new Float3(0.0f, wallHeight, 0.0f);
            rs.V4 = rs.V1 + new Float3(0.0f, wallHeight, 0.0f);
            var s = new Float2((float)imgW * 0.5f, (float)imgH * 0.5f);
            rs.P3 = trans.ProjectPoint(rs.V3).Scale(s) + s;
            rs.P4 = trans.ProjectPoint(rs.V4).Scale(s) + s;
            return rs;
        }
    }

    class Mesh : IDisposable
    {
        internal List<Quad> Quads = new List<Quad>();
        internal static Float2[] texCoords = new Float2[]
        {
            new Float2(1.0f, 0.0f), new Float2(0.0f, 0.0f), new Float2(0.0f, 1.0f), new Float2(1.0f, 1.0f)
        };
        internal List<Bitmap> Textures = new List<Bitmap>();
        internal Mesh(Bitmap image, List<Quad> quads, PerspectiveTransformation trans, float ceilHeight)
        {
            float sx = 1.0f / (float)image.Width;
            float sy = 1.0f / (float)image.Height;
            this.Quads.AddRange(quads);
            foreach (var q in quads)
            {
                var tex = Homography.Homography.RectifyImage(image,
                    new float[] { q.P2.x * sx, q.P3.x * sx, q.P4.x * sx, q.P1.x * sx },
                    new float[] { q.P2.y * sy, q.P3.y * sy, q.P4.y * sy, q.P1.y * sy });
                Textures.Add(tex);
            }
            float maxh = quads.Max(q => Math.Max(q.P3.y, q.P4.y));
            float minh = quads.Min(q => Math.Min(q.P1.y, q.P2.y));
            float ceilFarz = quads.Min(q => Math.Min(q.V3.z, q.V4.z));
            float ceilNearz = quads.Max(q => Math.Max(q.V3.z, q.V4.z));
            {
                // ground plane
                float groundFarz = quads.Min(q => Math.Min(q.V1.z, q.V2.z));
                float groundNearz = quads.Max(q => Math.Max(q.V1.z, q.V2.z));

                var sp1 = new Float2(0.0f, minh);
                var sp2 = new Float2(image.Width, minh);
                var hp1 = trans.UnprojectBasepoint(sp1);
                var hp2 = trans.UnprojectBasepoint(sp2);
                float nearZ = trans.UnprojectBasepoint(new Float2(0.0f, image.Height)).z;
                var hp3 = new Float3(hp2.x, hp2.y, nearZ);
                var hp4 = new Float3(hp1.x, hp1.y, nearZ);
                var s = new Float2((float)image.Width * 0.5f, (float)image.Height * 0.5f);
                var sp3 = trans.ProjectPoint(hp3).Scale(s) + s;
                var sp4 = trans.ProjectPoint(hp4).Scale(s) + s;
                var ground = Homography.Homography.RectifyImage(image,
                    new float[] { sp1.x * sx, sp4.x * sx, sp3.x * sx, sp2.x * sx },
                    new float[] { sp1.y * sy, sp4.y * sy, sp3.y * sy, sp2.y * sy });
                this.Quads.Add(new Quad() { V1 = hp2, V2 = hp1, V3 = hp4, V4 = hp3, P1 = sp1, P2 = sp4, P3 = sp3, P4 = sp2 });
                //ground.Save("d:\\ground.bmp");
                Textures.Add(ground);
            }
            {
                // ceil plane
                var sp1 = new Float2(0.0f, maxh);
                var sp2 = new Float2(image.Width, maxh);
                var hp1 = trans.UnprojectCeilpoint(sp1, ceilHeight);
                var hp2 = trans.UnprojectCeilpoint(sp2, ceilHeight);
                float nearZ = trans.UnprojectCeilpoint(new Float2(0.0f, 0.0f), ceilHeight).z;
                var hp3 = new Float3(hp2.x, hp2.y, nearZ);
                var hp4 = new Float3(hp1.x, hp1.y, nearZ);
                var s = new Float2((float)image.Width * 0.5f, (float)image.Height * 0.5f);
                var sp3 = trans.ProjectPoint(hp3).Scale(s) + s;
                var sp4 = trans.ProjectPoint(hp4).Scale(s) + s;
                var ceiling = Homography.Homography.RectifyImage(image,
                    new float[] { sp1.x * sx, sp4.x * sx, sp3.x * sx, sp2.x * sx },
                    new float[] { sp1.y * sy, sp4.y * sy, sp3.y * sy, sp2.y * sy });
                this.Quads.Add(new Quad() { V1 = hp2, V2 = hp1, V3 = hp4, V4 = hp3, P1 = sp1, P2 = sp4, P3 = sp3, P4 = sp2 });
                //ground.Save("d:\\ceiling.bmp");
                Textures.Add(ceiling);
            }
        }

        public void Dispose()
        {
            foreach (var bmp in Textures)
                bmp.Dispose();
        }
    }
    class ImageAnnotation
    {
        internal List<LineAnnotation> Lines = new List<LineAnnotation>();
        internal List<Tuple<LineAnnotation, LineAnnotation>> ParallelPairs = new List<Tuple<LineAnnotation, LineAnnotation>>();
        internal float WallHeight;
        internal float CameraHeight;
        internal float FocalLength;
        internal bool IsConsistent
        {
            get
            {
                return true;
                //return ParallelPairs.Count >= 1;
            }
        }
        internal PerspectiveTransformation GetTransform(int imgWidth, int imgHeight)
        {
            return new PerspectiveTransformation(Horizon, CameraHeight, FocalLength, imgWidth / 2, imgHeight / 2);
        }
        internal Mesh ToMesh(Bitmap img)
        {
            var trans = GetTransform(img.Width, img.Height);
            return new Mesh(img, GetQuads(img.Width, img.Height, trans), trans, WallHeight);
        }
        internal List<Quad> GetQuads(int imgWidth, int imgHeight, PerspectiveTransformation trans)
        {
            List<Quad> rs = new List<Quad>();
            foreach (var l in Lines)
            {
                if (l.IsAux)
                    continue;
                if (l.P1.X > l.P2.X || l.P1.X == l.P2.X && l.P1.Y < l.P2.Y)
                {
                    var p = l.P1;
                    l.P1 = l.P2;
                    l.P2 = p;
                }
                if (l.P1.X > 0 && l.P1.Y >= imgHeight)
                {
                    var dir = new Float2(l.P1) - new Float2(l.P2);
                    l.P1.Y -= (int)(dir.y / dir.x * l.P1.X);
                    l.P1.X = 0;
                }
                if (l.P2.X < imgWidth && l.P2.Y >= imgHeight)
                {
                    var dir = new Float2(l.P2) - new Float2(l.P1);
                    l.P2.Y += (int)(dir.y / dir.x * (imgWidth - l.P2.X));
                    l.P2.X = imgWidth;
                }
                rs.Add(l.ToQuad(trans, WallHeight, imgWidth, imgHeight));
            }
            return rs;
        }
        internal List<Quad> GetQuads(int imgWidth, int imgHeight)
        {
            var trans = new PerspectiveTransformation(Horizon, CameraHeight, FocalLength, imgWidth / 2, imgHeight / 2);
            return GetQuads(imgWidth, imgHeight, trans);
        }
        // returns horizon y coordinate in screen space
        internal float Horizon
        {
            get;
            set;
            //get
            //{
            //    return ParallelPairs.Average(p =>
            //    {
            //        var e1 = p.Item1.Equation;
            //        var e2 = p.Item2.Equation;
            //        float y = (e2.x * e1.z / e1.x - e2.z) / (e2.y - e2.x * e1.y / e1.x);
            //        return y;
            //    });
            //}
        }

        private void DrawWallQuads(ImageView view, Graphics g)
        {
            if (!IsConsistent)
                return;
            float horizon = Horizon;
            g.DrawLine(Pens.Yellow, view.ImageSpacePointToScreenSpacePoint(new Point(0, (int)horizon)),
                view.ImageSpacePointToScreenSpacePoint(new Point(view.Image.Width, (int)horizon)));
            var quads = GetQuads(view.Image.Width, view.Image.Height);
            foreach (var q in quads)
            {
                g.DrawLines(Pens.DarkRed, new Point[]
                { 
                    view.ImageSpacePointToScreenSpacePoint(q.P2.ToPoint()), 
                    view.ImageSpacePointToScreenSpacePoint(q.P3.ToPoint()), 
                    view.ImageSpacePointToScreenSpacePoint(q.P4.ToPoint()), 
                    view.ImageSpacePointToScreenSpacePoint(q.P1.ToPoint()) 
                });
            }
        }

        internal void Draw(ImageView view, Graphics g)
        {
            using (var dashPen = new Pen(Brushes.Blue))
            {
                dashPen.DashStyle = System.Drawing.Drawing2D.DashStyle.Dash;
                foreach (var l in Lines)
                {
                    var pen = Pens.Red;
                    if (l.IsAux)
                        pen = dashPen;
                    g.DrawLine(pen, view.ImageSpacePointToScreenSpacePoint(l.P1),
                        view.ImageSpacePointToScreenSpacePoint(l.P2));
                }
            }
            foreach (var p in ParallelPairs)
            {
                var center1 = view.ImageSpacePointToScreenSpacePoint(p.Item1.Center);
                var center2 = view.ImageSpacePointToScreenSpacePoint(p.Item2.Center);
                g.DrawLine(Pens.Green, center1, center2);
            }
            DrawWallQuads(view, g);
        }

        internal void DrawHighlightHandle(ImageView view, Graphics g, Point p)
        {
            var sp = view.ImageSpacePointToScreenSpacePoint(p);
            var handleRect = new Rectangle(sp.X - 3, sp.Y - 3, 6, 6);
            g.FillRectangle(Brushes.Yellow, handleRect);
        }

        internal void DrawLineHandles(ImageView view, Graphics g)
        {
            foreach (var l in Lines)
            {
                var center = view.ImageSpacePointToScreenSpacePoint(l.Center);
                var handleRect = new Rectangle(center.X - 3, center.Y - 3, 6, 6);
                g.FillRectangle(Brushes.Red, handleRect);
            }
        }

        internal LineAnnotation LinesHitTest(ImageView view, int x, int y)
        {
            foreach (var l in Lines)
            {
                var center = view.ImageSpacePointToScreenSpacePoint(l.Center);
                var handleRect = new Rectangle(center.X - 3, center.Y - 3, 6, 6);
                var mp = view.ImageSpacePointToScreenSpacePoint(new Point(x, y));
                if (handleRect.Contains(mp))
                    return l;
            }
            return null;
        }
    }
}
